#pragma once
#include <stdio.h>
#include <intrin.h>
#include <smmintrin.h>

#include<string>
#include<map>
#include<vector>

#include "Shape/PointCloud.h"
#include "Geometry/Vector3.h"
#include "Geometry/BoundingBox.h"
#include "System/Platform.h"

#include<Eigen/Dense>
#include <pcl/kdtree/kdtree_flann.h>
#include <boost/interprocess/file_mapping.hpp>
#include <boost/interprocess/mapped_region.hpp>

namespace PCS
{
	namespace Core
	{     	

		struct stat {
				int threadId;
				float elapsed;
		};

		struct cellElement {
				grid_key key;
				std::vector<size_t>* points;
		};

		class SparseGrid
		{
		protected:
			Vector3 m_size;											//number of cells along the three axis of the grid.
			Vector3 m_resolution;									//resolution along the X,Y,Z axis of the grid
			Vector3 m_extent;
			AxisAlignedBoundingBox * m_pBounds;						//bounding volume for the point cloud assigned to this space ... not necessarily the same as the point cloud volume.
			std::map<grid_key, std::vector<size_t>*> * m_pGrid;		//map data structure used to store points in cloud
			PointCloud * m_pointCloud;
			float m_buffer;											//expand this sparse grid bounding volume by this buffer 
			float m_neighbourRadiusSq;
			
			float m_extent_x, m_extent_y, m_extent_z;
			size_t totalPoints;

			int averageBinSize;
			int minBinSize;
			int maxBinSize;

			float r_average;

			size_t * r_addr;

		public:			
			SparseGrid(void) { m_resolution = 10.0f; m_buffer = 0.1f; m_pGrid = new std::map<grid_key, std::vector<size_t>*>(); }
			SparseGrid(float p_resolution) { m_resolution = p_resolution; m_buffer = 0.1f; m_pGrid = new std::map<grid_key, std::vector<size_t>*>(); }
			SparseGrid(Vector3 * p_resolution) { m_resolution = *p_resolution; m_buffer = 0.1f; m_pGrid = new std::map<grid_key, std::vector<size_t>*>(); }
			SparseGrid(float p_resolution, float p_buffer) { m_resolution = p_resolution; m_buffer = p_buffer; m_pGrid = new std::map<grid_key, std::vector<size_t>*>(); }

			~SparseGrid() 
			{
				//Delete cells in the grid
				//int idx = 0;
				//for (std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = m_pGrid->begin(); cellIterator != m_pGrid->end(); ++cellIterator)				
				//{					
					//cellIterator->second->clear()				
				//	delete cellIterator->second;
					//idx+=1;
				//}
				delete m_pGrid;
				delete m_pointCloud;
			}

			inline static unsigned int gridHash(int x, int y, int z) {
				return ((x & 0x3FF) << 20) | ((y & 0x3FF) << 10) | (z & 0x3FF);
			}

			/*inline static std::string gridHash(int x, int y, int z) {
				return boost::str(boost::format("%d:%d:%d") % xIdx % yIdx % zIdx)
			}*/

			/**Returns a vector of AABB enclosing the points within cells in the sparse grid*//*
			std::vector<AxisAlignedBoundingBox*> * GetInstrumentation() {
				std::vector<AxisAlignedBoundingBox*> * boxes = new std::vector<AxisAlignedBoundingBox*>();
				boxes->push_back(new AxisAlignedBoundingBox(*m_pBounds));
				float minx, miny, minz, maxx, maxy, maxz;
				for (std::map<grid_key, std::vector<Point*>*>::iterator cellIterator = m_pGrid->begin(); 
					 cellIterator != m_pGrid->end(); ++cellIterator)
				{							
					minx = miny = minz = 10000;
					maxx = maxy = maxz = -10000;
					for (std::vector<Point*>::iterator pointIterator = cellIterator->second->begin();
						pointIterator != cellIterator->second->end(); ++ pointIterator) {	
							if ((*pointIterator)->position.X < minx) minx = (*pointIterator)->position.X;
							if ((*pointIterator)->position.Y < miny) miny = (*pointIterator)->position.Y;
							if ((*pointIterator)->position.Z < minz) minz = (*pointIterator)->position.Z;

							if ((*pointIterator)->position.X > maxx) maxx = (*pointIterator)->position.X;
							if ((*pointIterator)->position.Y > maxy) maxy = (*pointIterator)->position.Y;
							if ((*pointIterator)->position.Z > maxz) maxz = (*pointIterator)->position.Z;
					}
					boxes->push_back(new AxisAlignedBoundingBox(Vector3(minx,miny,minz), Vector3(maxx,maxy,maxz)));
				}

				return boxes;
			}
			*/
			AxisAlignedBoundingBox * GetBounds() {
				return m_pBounds;
			}

			int GetAverageBinSize() {
				return averageBinSize;
			}

			int GetMinBinSize() {
				return minBinSize;
			}

			int GetMaxBinSize() {
				return maxBinSize;
			}

			Vector3 GetSize() {
				return m_size;
			}

			Vector3 GetResolution() {
				return m_resolution;
			}

			Vector3 GetExtent() {
				return m_extent;
			}

			int getTotalPoints() {
				return totalPoints;
			}

			std::map<grid_key, std::vector<size_t>*> * GetMap() {
				return m_pGrid;
			}

			PCS::Core::PointCloud * GetPointCloud() {
				return m_pointCloud;
			}			

			void SetBoundsWithBuffer(AxisAlignedBoundingBox * p_bounds) 
			{ 
				Vector3 gridBuffer = p_bounds->GetExtent() * 2 * m_buffer;
				m_pBounds = new AxisAlignedBoundingBox(p_bounds->GetMinExtent()-gridBuffer, p_bounds->GetMaxExtent()+gridBuffer); 
			} 

			void SetBoundsNoBuffer(AxisAlignedBoundingBox * p_bounds) 
			{ 
				m_pBounds = new AxisAlignedBoundingBox(p_bounds->GetMinExtent(), p_bounds->GetMaxExtent()); 
			} 								

			/**Generate the sparse grid structure and insert all point within. Then update point type.*/
			int Initialise(PointCloud* &p_cloud, float p_histogramDensity) 
			{				
				boost::timer timer;
				//boost::interprocess::file_mapping m_file("../../../../Media/PointClouds/MMF/pyramid1.mmf");

				m_pointCloud = p_cloud;

				SetBoundsWithBuffer((AxisAlignedBoundingBox*)(p_cloud->GetBoundingVolume()));
				m_extent.X = (m_pBounds->GetMaxExtent().X) - (m_pBounds->GetMinExtent().X); 
				m_extent.Y = (m_pBounds->GetMaxExtent().Y) - (m_pBounds->GetMinExtent().Y);
				m_extent.Z = (m_pBounds->GetMaxExtent().Z) - (m_pBounds->GetMinExtent().Z);	
				m_extent.W = 0.0f;

				//Now determine number of cells across the X, Y and Z dimensions. Static for now.
				m_size.X = m_extent.X / m_resolution.X;
				m_size.Y = m_extent.Y / m_resolution.Y;
				m_size.Z = m_extent.Z / m_resolution.Z;
				m_size.W = 0.0f;
				std::cout << "Number of cells along X, Y and Z = " << m_size.ToString() << std::endl;

				//Set neighbourdistance as the average of the resolutions and divide by 2 ... heuristic.
				m_neighbourRadiusSq = ((m_resolution.X + m_resolution.Y + m_resolution.Z) / 2) * ((m_resolution.X + m_resolution.Y + m_resolution.Z) / 2);				

				//Now iterate over the points in the cloud and insert them in their respective sgrid cells							
				std::vector<size_t>* v;
				std::map<grid_key, std::vector<size_t>*>::iterator gridIterator;

				int xIdx, yIdx, zIdx;				
				int xIdxOld, yIdxOld, zIdxOld;				
				int bins = 0;			
				r_average = 0.0f;
				averageBinSize = 0;
				minBinSize = 100;
				maxBinSize = 0;
				xIdxOld = yIdxOld = zIdxOld = -1000;
				__m128 a,b,c,d;

				b = _mm_load_ps(m_pBounds->GetMinExtent().Element);
				c = _mm_load_ps(m_size.Element);
				d = _mm_load_ps(m_extent.Element);																				
													
				size_t idx = 0;

				for (std::vector<Point*>::iterator pointIterator = p_cloud->GetPoints()->begin(); 
					 pointIterator != p_cloud->GetPoints()->end(); ++pointIterator, ++idx)
				{		
					//Implement the following using SSE intrinsics
					//xIdx = (int)floor((float)((((*pointIterator)->position.X - m_pBounds->GetMinExtent().X) * m_size.X) / m_extent.X));
					//yIdx = (int)floor((float)((((*pointIterator)->position.Y - m_pBounds->GetMinExtent().Y) * m_size.Y) / m_extent.Y));
					//zIdx = (int)floor((float)((((*pointIterator)->position.Z - m_pBounds->GetMinExtent().Z) * m_size.Z) / m_extent.Z));									

					a = _mm_load_ps((*pointIterator)->position.Element);																	
					a = _mm_sub_ps(a, b);
					a = _mm_mul_ps(a, c);
					a = _mm_div_ps(a, d);
					a = _mm_floor_ps(a);
					
					xIdx = a.m128_f32[0];
					yIdx = a.m128_f32[1];
					zIdx = a.m128_f32[2];					
					
					//If indices have not changed, insert in the same grid cell.
					if (xIdx==xIdxOld && yIdx == yIdxOld && zIdx == zIdxOld) {
						gridIterator->second->push_back(idx);
						if (gridIterator->second->size() > maxBinSize) maxBinSize = gridIterator->second->size();
						if (gridIterator->second->size() < minBinSize) minBinSize = gridIterator->second->size();
						//std::cout << "Adding to previous cell ... #" << gridIterator->second->size() << std::endl; 
						continue;
					}

					//Insert point in grid at the computed key.					
					if ((gridIterator = m_pGrid->find(gridHash(xIdx, yIdx, zIdx))) == m_pGrid->end())
					{
						//Create a new vector, insert this point and include in the hash map.																								
						v = new std::vector<size_t>();
						v->push_back(idx);
						(*m_pGrid)[gridHash(xIdx, yIdx, zIdx)] = v;
						gridIterator = m_pGrid->find(gridHash(xIdx, yIdx, zIdx));
						bins++;
						//std::cout << "Creating new cell ..." << xIdx << "," << yIdx << "," << zIdx << " -> " << gridHash(xIdx, yIdx, zIdx) << std::endl; 
					}
					else
					{
						//Retreive the vector attached to this grid cell and add the new point to it.
						gridIterator->second->push_back(idx);
						if (gridIterator->second->size() > maxBinSize) maxBinSize = gridIterator->second->size();
						if (gridIterator->second->size() < minBinSize) minBinSize = gridIterator->second->size();
						//std::cout << "Adding to cell ..." << xIdx << "," << yIdx << "," << zIdx << " -> " << gridHash(xIdx, yIdx, zIdx) << " #" << gridIterator->second->size() <<std::endl;
					}
					
					xIdxOld = xIdx;
					yIdxOld = yIdx;
					zIdxOld = zIdx;
				}

				totalPoints = p_cloud->GetSize();				
				averageBinSize = totalPoints / bins;
								
				std::cout << boost::str(boost::format("[SparseGrid] Populated in %ds. Bin# = %d, avg = %d max = %d min = %d.") % timer.elapsed() % bins % averageBinSize % maxBinSize % minBinSize) << std::endl;				
				
				if (p_histogramDensity != 0) PrintDensityHistogram(p_histogramDensity);
				UpdatePointType_DD();				
				 
				return 0;
			};

			void PrintDensityHistogram(float p_binDensity) {
				float binDensitySpan = GetMaxBinSize() - GetMinBinSize();
				float binWidth = binDensitySpan * p_binDensity;
				int numberOfBins = (int) (binDensitySpan / binWidth);
				std::vector<int> bins = std::vector<int>();
				for (int i = 0; i<numberOfBins+1; i++) bins.push_back(0);
				for (std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = m_pGrid->begin(); cellIterator != m_pGrid->end(); ++cellIterator) {					

					bins[(int)(cellIterator->second->size()/binWidth)]+=1;
				}
				std::cout << "[SparseGrid] Histogram ******" << std::endl; 
				for (int i = 0; i<numberOfBins+1; i++) {
					if (bins[i] != 0)
						std::cout << "[SparseGrid] Bin " << i << " [" << i*binWidth << "-" << (i*binWidth)+binWidth << "] = " << bins[i] << std::endl;
				}
				std::cout << "[SparseGrid] Histogram ******" << std::endl; 
			}

			/**Iterate over the points in each cell of the grid, setting their type (surface or edge)*/
			/*void UpdatePointType() {
				stat * stats = new stat[4];
				boost::timer timer;
				int neighbourCount = 0;
				int pointsChecked = 0;
				int notEnoughNeighbours = 0;
				
				int edgePoints = 0;
				int surfacePoints = 0;							
				
				int numberOfProcs = omp_get_num_procs();				
				//omp_set_num_threads(16);
				//numberOfProcs = 16;
				int portion = m_pGrid->size() / numberOfProcs;
				int portion2 = totalPoints / numberOfProcs;
				std::cout << "Setting Point Types ... [" << numberOfProcs << " threads] ..." << std::endl;
				
				std::vector<int> * map_splits = new std::vector<int>();
				map_splits->push_back(0);
				int portionIndex = 0;
				int iteratorIndex = 0;
				int currentTotal = 0;
				for (std::map<grid_key, std::vector<Point*>*>::iterator cellIterator = m_pGrid->begin(); cellIterator != m_pGrid->end(); ++cellIterator)				
				{
					iteratorIndex++;
					if (currentTotal > portion2) {
						map_splits->push_back(iteratorIndex);
						currentTotal = 0;
						portionIndex++;
					} else {
						currentTotal = currentTotal + cellIterator->second->size();
					}
				}
				
				#pragma omp parallel for schedule(guided)
				for (int i=0; i<numberOfProcs; i++) {
					float elapsedStart = timer.elapsed();
					int localNeighbourCount = 0;
					int localPointsChecked = 0;
					int localNotEnoughNeighbours = 0;
					int xIdxP; int yIdxP; int zIdxP;
					int localEdgePoints = 0;
					int localSurfacePoints = 0;

					Eigen::Matrix3f covariance = Eigen::Matrix3f();				
					Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver;
					
					std::vector<std::vector<Point*>*> neighbourCells = std::vector<std::vector<Point*>*>();
					std::vector<Point*> neighbours = std::vector<Point*>();
					std::vector<std::string> splitVec;
					std::string cellKey;
					std::map<grid_key, std::vector<Point*>*>::iterator gridIterator;
					std::map<grid_key, std::vector<Point*>*>::iterator cellIterator = m_pGrid->begin();
					std::map<grid_key, std::vector<Point*>*>::iterator cellIteratorEnd = m_pGrid->begin();					

					//Initialise both begin and end iterators ..					
					int start = 0; int end = 0; int threadID = omp_get_thread_num();
					for (int i=0; i<map_splits->at(threadID); ++i) { cellIterator++; start++; }										
					if (threadID == numberOfProcs-1) {						
						for (int i=0; i<m_pGrid->size()-1; ++i) { cellIteratorEnd++; end++; }
					} else { for (int i=0; i<map_splits->at(threadID+1) - 1; ++i) { cellIteratorEnd++; end++; } }
					
					for ( ; cellIterator != cellIteratorEnd; ++cellIterator)				
					{										
						//Determine which adjacent cells need to be considered and pass them to the neighbourhood query
						neighbourCells.clear();
						cellKey = (*cellIterator).first;
						boost::algorithm::split(splitVec, cellKey, boost::algorithm::is_any_of(":"), boost::algorithm::token_compress_on);					

						xIdxP = atoi(splitVec[0].c_str());
						yIdxP = atoi(splitVec[1].c_str());
						zIdxP = atoi(splitVec[2].c_str());

						for (int xIdx = xIdxP-1; xIdx < xIdxP+2; ++xIdx) {
							for (int yIdx = yIdxP-1; yIdx < yIdxP+2; ++yIdx) {
								for (int zIdx = zIdxP-1; zIdx < zIdxP+2; ++zIdx) {
									cellKey = boost::str(boost::format("%d:%d:%d") % xIdx % yIdx % zIdx);
									if ((gridIterator = m_pGrid->find(cellKey)) != m_pGrid->end())	{																
										neighbourCells.push_back((*gridIterator).second);
									}	
								}
							}							
						}

						for (std::vector<Point*>::iterator pointIterator = cellIterator->second->begin();
							pointIterator != cellIterator->second->end(); ++ pointIterator) {								
								GetNeighbours(*pointIterator, neighbourCells, neighbours, N_FOR_TYPE);							
								if (neighbours.size() < N_FOR_TYPE) notEnoughNeighbours++;
								else {
										CalcCovMatrix(neighbours, N_FOR_TYPE, covariance);							
										eigensolver.compute(covariance);									
										if (eigensolver.eigenvalues()[0] * CURVATURE > eigensolver.eigenvalues()[1]) {
											(*pointIterator)->T = edge;
											localEdgePoints++;
										} else { 
													(*pointIterator)->T = surface;
													localSurfacePoints++;
												}
								}
								localPointsChecked++;
						}						
					}

					//std::cout << boost::str(boost::format("[T%d - #=%d NotEnoughNeighbours=%d]") % omp_get_thread_num() % localPointsChecked % localNotEnoughNeighbours) << " done in " << timer.elapsed() << std::endl;
					stats[omp_get_thread_num()].threadId = omp_get_thread_num();
					stats[omp_get_thread_num()].elapsed = timer.elapsed() - elapsedStart;
					//write totals to shared variables
					#pragma omp critical(inc_patchid)
					{
						pointsChecked += localPointsChecked;
						edgePoints += localEdgePoints;
						surfacePoints += localSurfacePoints;
						notEnoughNeighbours += localNotEnoughNeighbours;
					}

				}  //end pragma
					
				for (int i = 0; i<4; i++) {
					std::cout << "T" << stats[i].threadId << " - " << stats[i].elapsed << std::endl;
				}

				std::cout << "done in " << timer.elapsed() << std::endl;
				std::cout << boost::str(boost::format("[#=%d E=%d S=%d NotEnoughNeighbours=%d]") % pointsChecked % edgePoints % surfacePoints % notEnoughNeighbours ) << std::endl;
			}
			*/
			/**Iterate over the points in each cell of the grid, setting their type (surface or edge)*/
			void UpdatePointType_DD() {
				stat * stats = new stat[4];
				cellElement * cellsInGrid = new cellElement[m_pGrid->size()];
				boost::timer timer;
				int neighbourCount = 0;
				int pointsChecked = 0;
				int notEnoughNeighbours = 0;
				
				int edgePoints = 0;
				int surfacePoints = 0;
				float eigenRatioEdge = 0.0f;

				int numberOfProcs = omp_get_num_procs();				
				//omp_set_num_threads(16);
				//numberOfProcs = 16;
				std::cout << "[SparseGrid] " << numberOfProcs << " threads starting to set point types"  << std::endl;
								
				//Initialise shared array of points to the cells in the sgrid				
				int idx = 0;
				for (std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = m_pGrid->begin(); cellIterator != m_pGrid->end(); ++cellIterator)				
				{
					cellsInGrid[idx].key = cellIterator->first;
					cellsInGrid[idx].points = cellIterator->second;
					idx+=1;
				}
								
				int size = idx;
				idx = 0;

				//Initialise Memeory Mapped File which will temporarily store neighbourhood query results.				
				//boost::interprocess::file_mapping::remove(FileNameQBuffer);
				int n_space = N_FOR_CRAWL+1;
				std::filebuf fbuf;
				fbuf.open(FileNameQBuffer, std::ios_base::in | std::ios_base::out | std::ios_base::trunc | std::ios_base::binary);
				fbuf.pubseekoff((totalPoints*n_space*sizeof(size_t))-sizeof(char), std::ios_base::beg);				
				fbuf.sputc(0);
				fbuf.close();

				boost::interprocess::file_mapping m_file(FileNameQBuffer, boost::interprocess::read_write);
				boost::interprocess::mapped_region region(m_file, boost::interprocess::read_write);						    

				r_addr = (size_t*)region.get_address();				
				size_t r_size = region.get_size();				
				std::cout << "[SparseGrid] MMF created and mapped to region. Size= " << r_size << std::endl;
				
				#pragma omp parallel for schedule(guided)
				for (int i=0; i<numberOfProcs; i++) {
					float elapsedStart = timer.elapsed();
					int localNeighbourCount = 0;
					int localPointsChecked = 0;
					int localNotEnoughNeighbours = 0;
					int xIdxP; int yIdxP; int zIdxP;
					int localEdgePoints = 0;
					int localSurfacePoints = 0;
					int increaseNeighbours = 0;
					int increaseInNeighbourhoodQueries = 0;
					bool incDone = false;
					int breakIncreaseNeighbours = 0;
					int pointsInBlock = 0;

					float minDistance = 100000;
					float maxDistance = 0;
					float crtDistance = 0;
					float medianDistance = 0;

					Eigen::Matrix3f covariance = Eigen::Matrix3f();				
					Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver;
					
					std::vector<std::vector<size_t>*> neighbourCells = std::vector<std::vector<size_t>*>();
					std::map<grid_key, std::vector<size_t>*> neighbourCellsMap = std::map<grid_key, std::vector<size_t>*>();
					std::vector<size_t> neighbours = std::vector<size_t>();	
					std::vector<size_t> pointList = std::vector<size_t>();
					grid_key cellKey;
					std::map<grid_key, std::vector<size_t>*>::iterator gridIterator;
					//std::vector<grid_key> nextPass = std::vector<std::string>();					
					
					int nextCell = 0;
					while (true) 
					{										
						//Determine which adjacent cells need to be considered and pass them to the neighbourhood query
						neighbourCells.clear();						
						nextCell = _InterlockedExchangeAdd(reinterpret_cast<volatile long*>(&idx), 1);
						if (nextCell > size-1) 
							break;
						cellKey = cellsInGrid[nextCell].key;
						gridIterator = m_pGrid->find(cellKey);
						
						//first search the voxel this point is located in						
						neighbourCells.push_back((*gridIterator).second);
						pointsInBlock = 0; //reset number of points in this block
						//then its adjacent ones
						unsigned int nck_x, nck_y;
						for (int xIdx = -1; xIdx < 2; ++xIdx) {
							nck_x = cellKey + (xIdx << 20);
							for (int yIdx = -1; yIdx < 2; ++yIdx) {
								nck_y = nck_x + (yIdx << 10);
								for (int zIdx = -1; zIdx < 2; ++zIdx) {
									if (!(xIdx==0 && yIdx==0 && zIdx==0)) {										
										if ((gridIterator = m_pGrid->find(nck_y+zIdx)) != m_pGrid->end())	{
											neighbourCells.push_back((*gridIterator).second);
											pointsInBlock+=(*gridIterator).second->size();											
										}
									}
								}
							}
						}
						
						if (pointsInBlock > KDTREE_THOLD) {
							//create KDTREE on the local set of points and use it for neighbourhood queries							
							pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_cloud (new pcl::PointCloud<pcl::PointXYZ>);
							pointList.clear();
							for (std::vector<std::vector<size_t>*>::iterator cellIterator = neighbourCells.begin(); cellIterator != neighbourCells.end(); ++cellIterator) {
								for (std::vector<size_t>::iterator pointIterator = (*cellIterator)->begin(); pointIterator != (*cellIterator)->end(); ++pointIterator) { 
									pcd_cloud->push_back(pcl::PointXYZ(m_pointCloud->GetPointAt(*pointIterator)->position.X, m_pointCloud->GetPointAt(*pointIterator)->position.Y, m_pointCloud->GetPointAt(*pointIterator)->position.Z)); 
									pointList.push_back(*pointIterator);
								}
							}
							pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
							kdtree.setInputCloud(pcd_cloud);
							std::vector<int> pointIdxNKNSearch(N_FOR_TYPE);
							std::vector<float> pointNKNSquaredDistance(N_FOR_TYPE);
							int idx=0;
							size_t crtIdx;
							for (std::vector<size_t>::iterator pointIterator = cellsInGrid[nextCell].points->begin();
								pointIterator != cellsInGrid[nextCell].points->end(); ++ pointIterator, ++ idx) {
									kdtree.nearestKSearch (idx, N_FOR_TYPE, pointIdxNKNSearch, pointNKNSquaredDistance);
									neighbours.clear();
									for (int i = 0; i<N_FOR_TYPE; i++) neighbours.push_back(pointList.at(pointIdxNKNSearch[i]));									
									//update MMF with neighbours data	
									crtIdx = *pointIterator;
									//std::cout << "Setting memory at base location " << crtIdx*n_space <<std::endl;
									neighbours.size() < N_FOR_CRAWL ? r_addr[crtIdx*n_space] = neighbours.size() :
																	  r_addr[crtIdx*n_space] = N_FOR_CRAWL;
									for (int nn = 1; nn<=neighbours.size() && nn<=N_FOR_CRAWL; nn++) { 
										//std::cout << "T" << omp_get_thread_num() << " Setting memory at offset location " << crtIdx*n_space+nn << " with nn=" << nn << " and npoint idx=" << pointList.at(pointIdxNKNSearch[nn]) <<std::endl;
										r_addr[crtIdx*n_space+nn] = pointList.at(pointIdxNKNSearch[nn]);
									}
									pointIdxNKNSearch.clear();
									pointNKNSquaredDistance.clear();
									if (neighbours.size() < N_FOR_TYPE) localNotEnoughNeighbours++;
									else {
										crtDistance = Vector3::DistanceSquared(m_pointCloud->GetPointAt(neighbours[N_FOR_TYPE-1])->position, m_pointCloud->GetPointAt(*pointIterator)->position);
										if (crtDistance > maxDistance) maxDistance = crtDistance;
										if (crtDistance < minDistance) minDistance = crtDistance;
										CalcCovMatrix(neighbours, N_FOR_TYPE, covariance);
										eigensolver.compute(covariance);
										if (eigensolver.eigenvalues()[0] * CURVATURE > eigensolver.eigenvalues()[1]) {											
												m_pointCloud->GetPointAt(*pointIterator)->T = edge;
												localEdgePoints++;											
										} else {
													//std::cout << "S- " << eigensolver.eigenvalues()[0] << "(" << eigensolver.eigenvalues()[0] * CURVATURE << ") - " << eigensolver.eigenvalues()[1] << eigensolver.eigenvalues()[2] << std::endl;
													m_pointCloud->GetPointAt(*pointIterator)->T = surface;
													localSurfacePoints++;													
												}
									}
									localPointsChecked++;
									if (localPointsChecked % 250000 == 0) 
										std::cout << boost::str(boost::format("[SparseGrid] T%d - #=%d") % omp_get_thread_num() % localPointsChecked) << " -> " << timer.elapsed() << std::endl;
							}							
							continue;
						}

						gridIterator = m_pGrid->find(cellKey);
						size_t crtIdx;
						for (std::vector<size_t>::iterator pointIterator = cellsInGrid[nextCell].points->begin();
							pointIterator != cellsInGrid[nextCell].points->end(); ++ pointIterator) {
								GetNeighbours(*pointIterator, neighbourCells, neighbours, N_FOR_TYPE);
								//update MMF with neighbours data	
								crtIdx = *pointIterator;
								//std::cout << "Setting memory at base location " << crtIdx*n_space <<std::endl;
								neighbours.size() < N_FOR_CRAWL ? r_addr[crtIdx*n_space] = neighbours.size() :
																  r_addr[crtIdx*n_space] = N_FOR_CRAWL;
								for (int nn = 1; nn<=neighbours.size() && nn<=N_FOR_CRAWL; nn++) { 
									//std::cout << "T" << omp_get_thread_num() << " Setting memory at offset location " << crtIdx*n_space+nn << " with nn=" << nn << " and npoint idx=" << pointList.at(pointIdxNKNSearch[nn]) <<std::endl;
									r_addr[crtIdx*n_space+nn] = neighbours[nn-1];
								}
								if (neighbours.size() < N_FOR_TYPE) localNotEnoughNeighbours++;
								else {
										crtDistance = Vector3::DistanceSquared(m_pointCloud->GetPointAt(neighbours[N_FOR_TYPE-1])->position, m_pointCloud->GetPointAt(*pointIterator)->position);
										if (crtDistance > maxDistance) maxDistance = crtDistance;
										if (crtDistance < minDistance) minDistance = crtDistance;
										CalcCovMatrix(neighbours, N_FOR_TYPE, covariance);
										eigensolver.compute(covariance);
										if (eigensolver.eigenvalues()[0] * CURVATURE > eigensolver.eigenvalues()[1]) {											
												m_pointCloud->GetPointAt(*pointIterator)->T = edge;
												localEdgePoints++;											
										} else {
													//std::cout << "S- " << eigensolver.eigenvalues()[0] << "(" << eigensolver.eigenvalues()[0] * CURVATURE << ") - " << eigensolver.eigenvalues()[1] << eigensolver.eigenvalues()[2] << std::endl;
													m_pointCloud->GetPointAt(*pointIterator)->T = surface;
													localSurfacePoints++;													
												}
								}
								localPointsChecked++;
								if (localPointsChecked % 250000 == 0) 
									std::cout << boost::str(boost::format("[SparseGrid] T%d - #=%d") % omp_get_thread_num() % localPointsChecked) << " -> " << timer.elapsed() << std::endl;
						}						
					}

					std::cout << boost::str(boost::format("[SparseGrid] T%d (%ds) #=%d NEN=%d MaxD=%d MinD=%d IncN=%d IncNQ=%d") % omp_get_thread_num() % timer.elapsed() % localPointsChecked % localNotEnoughNeighbours  % sqrt(maxDistance) % sqrt(minDistance) % increaseNeighbours % increaseInNeighbourhoodQueries) << std::endl;					
					stats[omp_get_thread_num()].threadId = omp_get_thread_num();
					stats[omp_get_thread_num()].elapsed = timer.elapsed() - elapsedStart;
					//write totals to shared variables
					#pragma omp critical(inc_patchid)
					{
						pointsChecked += localPointsChecked;
						edgePoints += localEdgePoints;
						surfacePoints += localSurfacePoints;
						notEnoughNeighbours += localNotEnoughNeighbours;
					}

				}  //end pragma									
				region.flush();
				std::cout << "[SparseGrid] All threads finished updating point types in " << timer.elapsed() << std::endl;				
				std::cout << boost::str(boost::format("[SparseGrid] Totals #=%d E=%d S=%d NEN=%d") % pointsChecked % edgePoints % surfacePoints % notEnoughNeighbours) << std::endl;
			}

			/**Returns points from the MMF populated in the UpdateDD function*/			
			void GetNeighbours(size_t p_fromPoint, std::vector<size_t> &p_neighbours, size_t * r_addr) {				
				p_neighbours.clear(); //Clear the vector				
				size_t numberOfNeighbours = r_addr[p_fromPoint*(N_FOR_CRAWL+1)];
				//std::cout<< "T" << omp_get_thread_num() << " Point Idx = " << p_fromPoint << " #Neighbours= " << numberOfNeighbours << std::endl;
				for (int i = 1, j=p_fromPoint*(N_FOR_CRAWL+1)+1; i<=numberOfNeighbours; i++, j++) p_neighbours.push_back(r_addr[j]);
			}
			
			/**Returns points within a "reasonable" distance from p_fromPoint located in this and surrounding voxels of the sparse grid*/			
			void GetNeighbours(Point * p_fromPoint, std::vector<std::vector<Point*>*> &p_cellValues, std::vector<Point*> &p_neighbours) {				
				p_neighbours.clear(); //Clear the vector
				for (std::vector<std::vector<Point*>*>::iterator cellIterator = p_cellValues.begin(); cellIterator != p_cellValues.end(); ++cellIterator) {
					for (std::vector<Point*>::iterator pointIterator = (*cellIterator)->begin(); pointIterator != (*cellIterator)->end(); ++pointIterator) {
						if (Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position) < m_neighbourRadiusSq) { 
							p_neighbours.push_back(*pointIterator);
						}	
					}
				}
			}						
			
			/**Returns the closest p_numberOfNeighbours from p_fromPoint located in the surrounding voxels of the sparse grid*/
			void GetNeighbours(size_t p_fromPoint, std::vector<std::vector<size_t>*> &p_cellValues, std::vector<Point*> &p_neighbours, int p_numberOfNeighbours) {				
				p_neighbours.clear(); //Clear the list
				std::vector<float> distances = std::vector<float>(p_numberOfNeighbours, 100); 				
				int distIdx = 0;
				float crtDist = 0.0f;				
				__m128 a,b;
				__m128 res;
				const int mask = 0xF8;				
				b = _mm_load_ps(m_pointCloud->GetPointAt(p_fromPoint)->position.Element);
				for (std::vector<std::vector<size_t>*>::iterator cellIterator = p_cellValues.begin(); cellIterator != p_cellValues.end(); ++cellIterator) {
					for (std::vector<size_t>::iterator pointIterator = (*cellIterator)->begin(); pointIterator != (*cellIterator)->end(); ++pointIterator) {												
						a = _mm_load_ps(m_pointCloud->GetPointAt(*pointIterator)->position.Element);												
						a = _mm_sub_ps(a, b);
						res = _mm_dp_ps(a, a, mask);
						crtDist = res.m128_f32[3];						
						if (crtDist < distances[p_numberOfNeighbours-1]) {
							for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
								if (crtDist <= distances[distIdx]) {								
									distances.insert(distances.begin()+distIdx, crtDist);									
									p_neighbours.insert(p_neighbours.begin()+distIdx, m_pointCloud->GetPointAt(*pointIterator));									
									break;
								}								
							}
						}						
					}
				}				
			}

			/**Returns the closest p_numberOfNeighbours from p_fromPoint located in the surrounding voxels of the sparse grid*/
			void GetNeighbours(size_t p_fromPoint, std::vector<std::vector<size_t>*> &p_cellValues, std::vector<size_t> &p_neighbours, int p_numberOfNeighbours) {				
				p_neighbours.clear(); //Clear the list
				std::vector<float> distances = std::vector<float>(p_numberOfNeighbours, 100); 				
				int distIdx = 0;
				float crtDist = 0.0f;				
				__m128 a,b;
				__m128 res;
				const int mask = 0xF8;				
				b = _mm_load_ps(m_pointCloud->GetPointAt(p_fromPoint)->position.Element);
				for (std::vector<std::vector<size_t>*>::iterator cellIterator = p_cellValues.begin(); cellIterator != p_cellValues.end(); ++cellIterator) {
					for (std::vector<size_t>::iterator pointIterator = (*cellIterator)->begin(); pointIterator != (*cellIterator)->end(); ++pointIterator) {												
						a = _mm_load_ps(m_pointCloud->GetPointAt(*pointIterator)->position.Element);												
						a = _mm_sub_ps(a, b);
						res = _mm_dp_ps(a, a, mask);
						crtDist = res.m128_f32[3];						
						if (crtDist < distances[p_numberOfNeighbours-1]) {
							for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
								if (crtDist <= distances[distIdx]) {								
									distances.insert(distances.begin()+distIdx, crtDist);									
									p_neighbours.insert(p_neighbours.begin()+distIdx, *pointIterator);									
									break;
								}								
							}
						}						
					}
				}				
			}

			/**Returns the closest p_numberOfNeighbours from p_fromPoint located in the surrounding voxels of the sparse grid*/
			/*void GetNeighboursOpt(Point * p_fromPoint, std::vector<Point*>* &p_cellPoints, std::map<std::string, std::vector<Point*>*> &p_cellValues, 
								  std::vector<Point*> &p_neighbours, int p_numberOfNeighbours, 
								  int xIdx, int yIdx, int zIdx, 
								  float &cell_min_x, float &cell_min_y, float &cell_min_z, float &cell_max_x, float &cell_max_y, float &cell_max_z) {				
				
				p_neighbours.clear(); //Clear the list
				std::vector<float> distances = std::vector<float>(p_numberOfNeighbours, 100); 				
				std::map<std::string, std::vector<Point*>*>::iterator neighboursIterator;
				int distIdx = 0;
				int numberOfNeighboursMinusOne = p_numberOfNeighbours-1;
				float crtDist = 0.0f;
				float posx = p_fromPoint->position.X;
				float posy = p_fromPoint->position.Y;
				float posz = p_fromPoint->position.Z;

				__m128 a,b;
				__m128 res;
				const int mask = 0xF8;				
				b = _mm_load_ps(p_fromPoint->position.Element);

				for (std::vector<Point*>::iterator pointIterator = p_cellPoints->begin(); pointIterator != p_cellPoints->end(); pointIterator++) {
					a = _mm_load_ps((*pointIterator)->position.Element);												
					a = _mm_sub_ps(a, b);
					res = _mm_dp_ps(a, a, mask);
					crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
					if (crtDist < distances[numberOfNeighboursMinusOne]) {
						for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
							if (crtDist <= distances[distIdx]) {								
								distances.insert(distances.begin()+distIdx, crtDist);									
								p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
								break;
							}								
						}
					}
				}

				float dist_from_min_x = (posx - cell_min_x) * (posx - cell_min_x);
				float dist_from_max_x = (cell_max_x - posx) * (cell_max_x - posx);
				float dist_from_min_y = (posy - cell_min_y) * (posy - cell_min_y);
				float dist_from_max_y = (cell_max_y - posy) * (cell_max_y - posy);
				float dist_from_min_z = (posz - cell_min_z) * (posz - cell_min_z);
				float dist_from_max_z = (cell_max_z - posz) * (cell_max_z - posz);
								
				//first check whether we are well inside the cell ... i.e. no points from adjacent cells can be neigbours
				if (dist_from_min_x > distances[numberOfNeighboursMinusOne] && 
					dist_from_max_x > distances[numberOfNeighboursMinusOne] &&
					dist_from_min_y > distances[numberOfNeighboursMinusOne] && 
					dist_from_max_y > distances[numberOfNeighboursMinusOne] &&
					dist_from_min_z > distances[numberOfNeighboursMinusOne] && 
					dist_from_max_z > distances[numberOfNeighboursMinusOne]) return;

				//...ok we're close to some border ... iterate over adjacent cells. Consider 27 individual conditions.
				std::string cellKey;
				//check directly adjacent cells first ... 6 of them
				//top 
				if (dist_from_max_y < distances[numberOfNeighboursMinusOne]) 
				{
					//yIdx = yIdx+1;
					cellKey = boost::str(boost::format("%d:%d:%d") % xIdx % (yIdx+1) % zIdx);
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}

				//bottom
				if (dist_from_min_y < distances[numberOfNeighboursMinusOne])
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % xIdx % (yIdx-1) % zIdx);
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}

				//left
				if (dist_from_min_x < distances[numberOfNeighboursMinusOne])					
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx-1) % yIdx % zIdx);
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}

				//right
				if (dist_from_max_x < distances[numberOfNeighboursMinusOne])					
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx+1) % yIdx % zIdx);
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}

				//front
				if (dist_from_min_z < distances[numberOfNeighboursMinusOne])					
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % xIdx % yIdx % (zIdx-1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}

				//back
				if (dist_from_max_z < distances[numberOfNeighboursMinusOne])					
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % xIdx % yIdx % (zIdx+1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}
				//Check cells on edges ... potentially 12 of them
				//Top Back Edge
				if (dist_from_max_y < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % xIdx % (yIdx+1) % (zIdx+1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}
				//Top Front Edge
				if (dist_from_max_y < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % xIdx % (yIdx+1) % (zIdx-1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}
				//Bottom Front Edge
				if (dist_from_min_y < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % xIdx % (yIdx-1) % (zIdx-1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}
				//Bottom Back Edge
				if (dist_from_min_y < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % xIdx % (yIdx-1) % (zIdx+1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}
				//Right Top Edge
				if (dist_from_max_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_y < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx+1) % (yIdx+1) % zIdx);
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}
				//Right Bottom Edge
				if (dist_from_max_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_y < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx+1) % (yIdx-1) % zIdx);
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}
				//Left Bottom Edge
				if (dist_from_min_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_y < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx-1) % (yIdx-1) % zIdx);
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}
				//Left Top Edge
				if (dist_from_min_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_y < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx-1) % (yIdx+1) % zIdx);
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}
				//Right Back Edge
				if (dist_from_max_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx+1) % yIdx % (zIdx+1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}
				//Left Back Edge
				if (dist_from_min_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx-1) % yIdx % (zIdx+1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}
				//Left Front Edge
				if (dist_from_min_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx-1) % yIdx % (zIdx-1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}
				//Right Front Edge
				if (dist_from_max_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx+1) % yIdx % (zIdx-1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}

				//Finally Check the 8 cells at corners
				//Left Bottom Front Corner
				if (dist_from_min_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_y < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx-1) % (yIdx-1) % (zIdx-1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}

				//Right Top Back
				if (dist_from_max_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_y < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx+1) % (yIdx+1) % (zIdx+1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}

				//Right Bottom Front
				if (dist_from_max_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_y < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx+1) % (yIdx-1) % (zIdx-1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}

				//Left Top Front
				if (dist_from_min_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_y < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx-1) % (yIdx+1) % (zIdx-1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}

				//Right Top Front
				if (dist_from_max_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_y < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx+1) % (yIdx+1) % (zIdx-1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}

				//Right Bottom Back
				if (dist_from_max_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_y < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx+1) % (yIdx-1) % (zIdx+1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}

				//Left Top Back
				if (dist_from_min_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_y < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx-1) % (yIdx+1) % (zIdx+1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}

				//Left Bottom Back
				if (dist_from_min_x < distances[numberOfNeighboursMinusOne] &&
					dist_from_min_y < distances[numberOfNeighboursMinusOne] &&
					dist_from_max_z < distances[numberOfNeighboursMinusOne]) 
				{
					cellKey = boost::str(boost::format("%d:%d:%d") % (xIdx-1) % (yIdx-1) % (zIdx+1));
					if ((neighboursIterator = p_cellValues.find(cellKey)) != p_cellValues.end()) {
						for (std::vector<Point*>::iterator pointIterator = neighboursIterator->second->begin(); pointIterator != neighboursIterator->second->end(); pointIterator++) {
							a = _mm_load_ps((*pointIterator)->position.Element);												
							a = _mm_sub_ps(a, b);
							res = _mm_dp_ps(a, a, mask);
							crtDist = res.m128_f32[3]; //==Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position);
					
							if (crtDist < distances[numberOfNeighboursMinusOne]) {
								for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
									if (crtDist <= distances[distIdx]) {								
										distances.insert(distances.begin()+distIdx, crtDist);									
										p_neighbours.insert(p_neighbours.begin()+distIdx, (*pointIterator));									
										break;
									}								
								}
							}
						}
					}
				}
					
			}
			*/
			/**Calculates the covariance matrix of the first p_numberOfNeighbours points in p_from. Stores it in p_result*/
			void CalcCovMatrix(std::vector<size_t> &p_from, int p_numberOfNeighbours, Eigen::Matrix3f &p_result)
			{				
				float c00, c11, c22, c01, c02, c12;
				c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;
				std::vector<size_t>::iterator start = p_from.begin();
				std::vector<size_t>::iterator end;

				if (p_from.size() > p_numberOfNeighbours) end = p_from.begin()+p_numberOfNeighbours;
				else end = p_from.end();

				//First Get Median Vector
				Vector3 median = Vector3(0);
								
				for (std::vector<size_t>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					median.X += (m_pointCloud->GetPointAt(*pointIterator))->position.X;
					median.Y += (m_pointCloud->GetPointAt(*pointIterator))->position.Y;
					median.Z += (m_pointCloud->GetPointAt(*pointIterator))->position.Z;
				}
				median /= p_numberOfNeighbours;

				//Now calculate diagonal of cov matrix
				for (std::vector<size_t>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					c00 += ((m_pointCloud->GetPointAt(*pointIterator))->position.X - median.X) * ((m_pointCloud->GetPointAt(*pointIterator))->position.X - median.X);
					c11 += ((m_pointCloud->GetPointAt(*pointIterator))->position.Y - median.Y) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Y - median.Y);
					c22 += ((m_pointCloud->GetPointAt(*pointIterator))->position.Z - median.Z) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Z - median.Z);
					c01 += ((m_pointCloud->GetPointAt(*pointIterator))->position.X - median.X) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Y - median.Y);
					c02 += ((m_pointCloud->GetPointAt(*pointIterator))->position.X - median.X) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Z - median.Z);
					c12 += ((m_pointCloud->GetPointAt(*pointIterator))->position.Y - median.Y) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Z - median.Z);
				}				

				p_result << c00 / p_numberOfNeighbours, c01 / p_numberOfNeighbours, c02 / p_numberOfNeighbours, 
							c01 / p_numberOfNeighbours, c11 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, 
							c02 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, c22 / p_numberOfNeighbours;
								
			}

			void CalcCovMatrix(std::vector<Point*> &p_from, int p_numberOfNeighbours, Eigen::Matrix3f &p_result)
			{				
				float c00, c11, c22, c01, c02, c12;
				c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;
				std::vector<Point*>::iterator start = p_from.begin();
				std::vector<Point*>::iterator end;

				if (p_from.size() > p_numberOfNeighbours) end = p_from.begin()+p_numberOfNeighbours;
				else end = p_from.end();

				//First Get Median Vector
				Vector3 median = Vector3(0);
								
				for (std::vector<Point*>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					median.X += (*pointIterator)->position.X;
					median.Y += (*pointIterator)->position.Y;
					median.Z += (*pointIterator)->position.Z;
				}
				median /= p_numberOfNeighbours;

				//Now calculate diagonal of cov matrix
				for (std::vector<Point*>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					c00 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.X - median.X);
					c11 += ((*pointIterator)->position.Y - median.Y) * ((*pointIterator)->position.Y - median.Y);
					c22 += ((*pointIterator)->position.Z - median.Z) * ((*pointIterator)->position.Z - median.Z);
					c01 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.Y - median.Y);
					c02 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.Z - median.Z);
					c12 += ((*pointIterator)->position.Y - median.Y) * ((*pointIterator)->position.Z - median.Z);
				}				

				p_result << c00 / p_numberOfNeighbours, c01 / p_numberOfNeighbours, c02 / p_numberOfNeighbours, 
							c01 / p_numberOfNeighbours, c11 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, 
							c02 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, c22 / p_numberOfNeighbours;
								
			}

			void CalcCovMatrix(std::vector<Point*> &p_from0, std::vector<Point*> &p_from1, std::vector<Point*> &p_from2, int p_numberOfNeighbours, Eigen::Matrix3f &p_result)
			{				
				float c00, c11, c22, c01, c02, c12;
				c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;
				std::vector<Point*>::iterator pointIterator;
				std::vector<Point*>::iterator start = p_from1.begin();
				std::vector<Point*>::iterator end;

				if (p_from1.size() > p_numberOfNeighbours) end = p_from1.begin()+p_numberOfNeighbours;
				else end = p_from1.end();
				for (pointIterator = start; pointIterator != end; ++pointIterator) {
					p_from0.push_back(*pointIterator);
				}

				start = p_from2.begin();
				if (p_from2.size() > p_numberOfNeighbours) end = p_from2.begin()+p_numberOfNeighbours;
				else end = p_from2.end();
				for (pointIterator = start; pointIterator != end; ++pointIterator) {
					p_from0.push_back(*pointIterator);
				}
							
				start = p_from0.begin();
				if (p_from0.size() > p_numberOfNeighbours*3) end = p_from0.begin()+(p_numberOfNeighbours*3);
				else end = p_from0.end();

				//First Get Median Vector
				Vector3 median = Vector3(0);
				int pointCount = 0;
								
				for (pointIterator = start; pointIterator != end; ++pointIterator) {
					median.X += (*pointIterator)->position.X;
					median.Y += (*pointIterator)->position.Y;
					median.Z += (*pointIterator)->position.Z;
					pointCount++;
				}

				median /= pointCount;

				//Now calculate diagonal of cov matrix
				for (std::vector<Point*>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					c00 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.X - median.X);
					c11 += ((*pointIterator)->position.Y - median.Y) * ((*pointIterator)->position.Y - median.Y);
					c22 += ((*pointIterator)->position.Z - median.Z) * ((*pointIterator)->position.Z - median.Z);
					c01 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.Y - median.Y);
					c02 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.Z - median.Z);
					c12 += ((*pointIterator)->position.Y - median.Y) * ((*pointIterator)->position.Z - median.Z);
				}				

				p_result << c00 / p_numberOfNeighbours, c01 / p_numberOfNeighbours, c02 / p_numberOfNeighbours, 
							c01 / p_numberOfNeighbours, c11 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, 
							c02 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, c22 / p_numberOfNeighbours;
								
			}

		};		

	}
}

/*if (gridIterator->second->size() > 400) {
							nextPass.push_back(cellKey);
							continue;
						}*/

						/*cell_min_x = m_pBounds->GetMinExtent().X + (xIdxP * m_resolution.X);
						cell_max_x = cell_min_x + m_resolution.X;
						cell_min_y = m_pBounds->GetMinExtent().Y + (yIdxP * m_resolution.Y);
						cell_max_y = cell_min_y + m_resolution.Y;
						cell_min_z = m_pBounds->GetMinExtent().Z + (zIdxP * m_resolution.Z);
						cell_max_z = cell_min_z + m_resolution.Z;
						*/

		//This is probably an edge ... but let's verify by increasing the number of neighbours first.
											//std::cout << "E- " << eigensolver.eigenvalues()[0] << "(" << eigensolver.eigenvalues()[0] * CURVATURE << ") - " << eigensolver.eigenvalues()[1] << eigensolver.eigenvalues()[2] << std::endl;
											//medianDistance = (maxDistance + minDistance) / 20;
											//int n = 2;
											//while (crtDistance < medianDistance) {
												//get more neighbours ... and recalculate
												//if (!incDone) increaseNeighbours++;
												//incDone=true;
												//increaseInNeighbourhoodQueries++;
												//GetNeighbours(*pointIterator, neighbourCells, neighbours, N_FOR_TYPE);
												//GetNeighbours(neighbours[(N_FOR_TYPE) -1], neighbourCells, neighbours1, N_FOR_TYPE);
												//GetNeighbours(neighbours[(N_FOR_TYPE) -2], neighbourCells, neighbours2, N_FOR_TYPE);
												//CalcCovMatrix(neighbours, neighbours1, neighbours2, N_FOR_TYPE, covariance);
												//eigensolver.compute(covariance);
												//if (eigensolver.eigenvalues()[0] * CURVATURE <= eigensolver.eigenvalues()[1]) break;												
												//crtDistance = Vector3::DistanceSquared(neighbours[(N_FOR_TYPE*n) -1]->position, (*pointIterator)->position);
												//if (neighbours.size() < N_FOR_TYPE) { breakIncreaseNeighbours++; break; }
												//n++;
											//}
											//incDone=false;
											//CalcCovMatrix(neighbours, N_FOR_TYPE*n, covariance);
											//eigensolver.compute(covariance);
											//if (eigensolver.eigenvalues()[0] * CURVATURE > eigensolver.eigenvalues()[1]) {
									